/*
 * World.cpp
 *
 *  Created on: May 17, 2011
 *      Author: madboyka
 */

#include "World.h"
#include "../common/PluginManager.h"
#include "MapLoader.h"
#include "pal/palFactory.h"
#include "pal/ConfigStatic.h"
#include "pal/palStatic.h"
#include "Logger.h"

#define SIMULATION_SPEED 1000000

World::World()
{
//	simulationStep = 1000000/60;
	simulationStep = 0.017;
	simulationRunning = false;
	initialized = false;

}

void World::init(Server* server)
{
	this->server = server;
#ifndef PAL_STATIC
	PF->LoadPALfromDLL();
#endif

	QString engineName = Server::getSetting("PhysicsEngine","ODE").toString();
	Server::getConsole()->log("Using engine %s.",engineName.toStdString().data());
	PF->SelectEngine(engineName.toStdString().data()); // Here is the name of the physics engine you wish to use. You could replace DEFAULT_ENGINE with "Tokamak", "ODE", etc...

	physics = PF->CreatePhysics();
	if (physics == NULL)
	{
		Logger::error("Failed to create the physics engine. "
			"Check to see if you spelled the engine name correctly,"
			" or that the engine DLL is in the right location\n");
		Server::kill();
	}
	//TODO read physics description from map
	palPhysicsDesc pdesc;
	physics->Init(pdesc);
	palTerrainPlane *pt = PF->CreateTerrainPlane(); //create the groun
	pt->Init(0, 0, 0, 50.0f); //initialize it, set its location to 0,0,0 and minimum size to 50
//	palBox *wall1 = PF->CreateBox();
//	wall1->Init(-50.0f, 5.0f, 0, 10.0f, 10.0f, 50.0f,100);
//	wall1->SetOrientation(0,0.5,0);
//	palBox *wall2 = PF->CreateBox();
//	wall2->Init(50.0f, 5.0f, 0, 10.f, 10.0f, 50.0f,100);
//	wall2->SetOrientation(0,0.5,0);
//	palBox *wall3 = PF->CreateBox();
//	wall3->Init(0, 5.0f, -50.0f, 50.0f, 10.0f, 10.0f,100);
//	wall3->SetOrientation(0,0.5,0);
//	palBox *wall4 = PF->CreateBox();
//	wall4->Init(0, 5.0f, 50.0f, 50.0f, 10.0f, 10.0f,100);
//	wall4->SetOrientation(0,0,0);
	/*palStaticBox *wall1 = static_cast<palStaticBox *> PF->CreateObject("palStaticBox");
	wall1->Init(-25.0f, 5.0f, 0, 10.0f, 10.0f, 50.0f);
	palBox *wall2 = PF->CreateBox();
	wall2->Init(25.0f, 5.0f, 0, 10.f, 10.0f, 50.0f,1000);
	palBox *wall3 = PF->CreateBox();
	wall3->Init(0, 5.0f, -25.0f, 50.0f, 10.0f, 10.0f,1000);
	palBox *wall4 = PF->CreateBox();
	wall4->Init(0, 5.0f, 25.0f, 50.0f, 10.0f, 10.0f,1000);*/
	PluginManager* pm = new PluginManager();
	pm->loadPlugin("../robotslib/plugin.so");
	obstacleFactory = pm->getObstacleFactory();
	robotFactory = pm->getRobotFactory();
	Logger::log("Got factories...");
	//	obstacles = new QVector<Obstacle>();
	//	robots = new QVector<Robot>();
	//	Obstacle* obs = addObstacle("Box");
	//	obs->setPosition(5, 50, 5);
	//	obs->createPhysics();
	//	obs = addObstacle("Box");
	//	obs->setPosition(5.8, 0.5, 5.8);
	//	obs->createPhysics();
	//	palBox * box = PF->CreateBox();
	//	box->Init(5.75, 1, 5.75,1,1,1,1);
	MapLoader* ml = new MapLoader();
	QString mapFile = Server::getSetting("MapFilePath","empty.xml").toString();
	Logger::log("Loading map file: %s\n",mapFile.toStdString().data());
	ml->load(mapFile);
	ml->addObjects(this);
	delete ml;
//	Obstacle* obs1 = addObstacle("Box");
//	obs1->setPosition(-50.0,5.0,0.0);
//	obs1->createPhysics();
//
//
//	Obstacle* obs2 = addObstacle("Box");
//	obs2->setPosition(50.0,5.0,0.0);
//	obs2->createPhysics();
//
//
//	Obstacle* obs3 = addObstacle("Box");
//	obs3->setPosition(0.0,5.0,-50.0);
//	obs3->createPhysics();
//
//
//	Obstacle* obs4 = addObstacle("Box");
//	obs4->setPosition(0.0,5.0,50.0);
//	obs4->createPhysics();

	initialized = true;
}

World::~World()
{
	// TODO Auto-generated destructor stub
}

void World::stepSimulation()
{
	physics->Update(simulationStep);
//	Server::log("Simulating...");
	requestElementsAccess();
	elements.clear();
//	printf("Simulation: %d obstacle, %d robots \n", obstacles.count(),
//			robots.count());
	QVectorIterator<Obstacle*> oit(obstacles);
	while (oit.hasNext())
	{
		WorldObject *obj = oit.next();
		loadLocations(obj);
	}
	QVectorIterator<Robot*> rit(robots);
	while (rit.hasNext())
	{
		WorldObject *obj = rit.next();
		loadLocations(obj);
	}
	restoreElementsAccess();
}

void World::loadLocations(WorldObject* obj)
{
	int count = obj->getElementCount();
	for (int i = 0; i < count; ++i)
	{
		ElementData edata(obj->getElementData(i));
		Logger::logMatrix(edata.getTransformationMatrix());
//		Logger::log("Simulation:: %d",edata.getElementType());
		elements.append(edata);
	}
}

void World::pauseSimulation()
{
}

void World::startSimulation()
{
	simulationRunning = true;
}

void World::run()
{
	while (1)
	{
		if (simulationRunning)
		{
			stepSimulation();
		}
		else
		{
		}
		int sleepTime = (int) (simulationStep * SIMULATION_SPEED);
		usleep(sleepTime);
	}
}

void World::reset()
{
	//delete physics;
}

Robot* World::getRobot(int i)
{
	return robots.at(i);
}

Obstacle* World::getObstacle(int i)
{
	return obstacles.at(i);
}

float World::getSimulationTime()
{
	return physics->GetTime();
}

float World::getStepTime()
{
	return simulationStep;
}

Robot* World::addRobot(const char* name)
{
	Robot *robot = robotFactory->create(name);
	robots.append(robot);
	return robot;
}

Obstacle* World::addObstacle(const char* name)
{
	Obstacle *obj = obstacleFactory->create(name);
	obstacles.append(obj);
	return obj;
}

void World::activateRobot(Robot* robot)
{
	robot->createPhysics();
	QVectorIterator<ViewerWorker*> it(server->viewers);
	while (it.hasNext())
	{
		ViewerWorker* vw = it.next();
		for (int i = 0; i < robot->getElementCount(); ++i)
		{
			vw->addNewElement(robot->getElementData(i));
		}
	}
}

void World::activateObstacle(Obstacle* obstacle)
{
	obstacle->createPhysics();
	QVectorIterator<ViewerWorker*> it(server->viewers);
	while (it.hasNext())
	{
		ViewerWorker* vw = it.next();
		for (int i = 0; i < obstacle->getElementCount(); ++i)
		{
			vw->addNewElement(obstacle->getElementData(i));
		}
	}
}

bool World::isSimulationRunning()
{
	return simulationRunning;
}

void World::initializeViewer(ViewerWorker *viewer)
{
	QVectorIterator<Obstacle*> oit(obstacles);
	while (oit.hasNext())
	{
		Obstacle* obstacle = oit.next();
		for (int i = 0; i < obstacle->getElementCount(); ++i)
		{
			viewer->addNewElement(obstacle->getElementData(i));
		}
	}
	QVectorIterator<Robot*> rit(robots);
	while (rit.hasNext())
	{
		Robot* robot = rit.next();
		for (int i = 0; i < robot->getElementCount(); ++i)
		{
			viewer->addNewElement(robot->getElementData(i));
		}
	}
}

void World::requestElementsAccess()
{
//	Server::log("Locking elements");
	elementsMutex.lock();
}



void World::restoreElementsAccess()
{
//	Server::log("Releasing elements");
	elementsMutex.unlock();
}


